#include "cpp_co_location_client/DDXTJC.hpp"

/**
 * @brief 计算单位向量
 *
 * 根据给定的相机参数和像素坐标，计算对应的单位向量。
 *
 * @param camera 相机参数
 * @param u 像素坐标的u值
 * @param v 像素坐标的v值
 *
 * @return 计算得到的单位向量
 */
Eigen::Vector3d DDXTJC::calculate_unit_vector(DDXTJC_Camera camera, int u, int v){
    
    Eigen::Vector3d unit_vector;
    unit_vector.x() = (u - camera.center_u) / camera.gamma_u;
    unit_vector.y() = (v - camera.center_v) / camera.gamma_v; 
    unit_vector.z() = 1;
    unit_vector =  unit_vector.normalized();
    return unit_vector;
}

Eigen::Vector3d DDXTJC::calculate_unit_vector(Gimbal_Camera camera, Picture_Location picture_loc){
    // 专利公式
    // 将相机视为针孔相机模型，计算从相机光心到目标的视轴线单位向量在相机坐标系下的表示
    Eigen::Vector3d unit_vector;
    unit_vector(0) = picture_loc.u;
    unit_vector(1) = picture_loc.v;
    // unit_vector(2) = camera.f / camera.d;
    unit_vector(2) = camera.fx;
    unit_vector =  unit_vector.normalized();
    return unit_vector;
}


std::vector<Eigen::Vector3d> DDXTJC::calculate_unit_vectors(DDXTJC_Camera camera,Eigen::VectorXd Z){
    int num_of_measurement = Z.rows();
    std::vector<Eigen::Vector3d> unit_vectors;
    for (int i = 0; i < num_of_measurement / 2; ++i) {
        auto value = calculate_unit_vector(camera,Z(2 * i), Z(2 * i + 1));
        unit_vectors.push_back(value);
    }
    return unit_vectors;
}


// 同Rb_a a坐标基座几何中心到b中的旋转关系得到 L_W_i W坐标系下第i个观测轴的单位向量的表示
// L_W_i = RW_Ni * RN_Bi * RB_Gi * RG_Ci * L_C_i
//  RW_Ni 为单位矩阵
//  RN_Bi 为无人机的欧拉角
//  RB_Gi 单位矩阵
//  RG_Ci 为相机坐标系下的旋转矩阵


/**
 * @brief 计算世界坐标系下的观测轴单位向量
 *
 * 根据给定的相机坐标系下的向量、全局旋转角和无人机旋转角，计算世界坐标系下的观测轴单位向量。
 *
 * @param v_C_i 相机坐标系下的向量
 * @param yaw_g 云台偏航角（单位：弧度）
 * @param pitch_g 云台俯仰角（单位：弧度）
 * @param roll_u 无人机滚转角（单位：弧度）
 * @param pitch_u 无人机俯仰角（单位：弧度）
 * @param yaw_u 无人机偏航角（单位：弧度）
 *
 * @return 世界坐标系下的观测轴单位向量
 */
Eigen::Vector3d DDXTJC::calculateWorldViewingAxisUnitVector(
    const Eigen::Vector3d& v_C_i,
    Eigen::MatrixXd CoRotationMatrix) {

    // Eigen::Matrix3d R_g = getRotationMatrixYaw(yaw_g) * getRotationMatrixPitch(pitch_g);
   
    // Eigen::Matrix3d R_u = getRotationMatrixYaw(yaw_u) * getRotationMatrixPitch(pitch_u) * getRotationMatrixRoll(roll_u);
    
    // Eigen::Matrix3d R = R_u * R_g;
    
    Eigen::Vector3d v_W_i = CoRotationMatrix * v_C_i;

    return v_W_i.normalized();
}

DDXTJC::DDXTJC(/* args */){
    // std::vector<std::string> model{"CV","CT","CA"};
    // int state_v = 10;
    // int mean = 6;
    // Eigen::MatrixXd P = Eigen::MatrixXd::Identity(state_v, state_v) *0.01;

    // ukf_ptr = std::make_shared<IMM_UKF>(model,state_v,mean,P);

    // finished = false;

}

DDXTJC::~DDXTJC(){

}


/**
 * @brief 计算世界坐标系下目标点的位置
 *
 * 根据给定的相机位置和单位向量，使用 Ceres Solver 库计算世界坐标系下目标点的位置。
 *
 * @param CameraPosition 相机位置向量集合
 * @param unitVectors 单位向量集合
 *
 * @return 目标点在世界坐标系下的位置
 */
Eigen::Vector3d DDXTJC::calculatorActorPosition(std::vector<Eigen::Vector3d> CameraPosition,std::vector<Eigen::Vector3d> unitVectors){
    
    double target_position[3] = {0.0, 0.0, 0.0};
    // ceres::Problem problem;

    // for (size_t i = 0; i < CameraPosition.size(); ++i) {
    //     ceres::CostFunction* cost_function = 
    //         new ceres::AutoDiffCostFunction<DistanceResidual, 1, 3>(
    //             new DistanceResidual(CameraPosition[i], unitVectors[i]));
    //     problem.AddResidualBlock(cost_function, nullptr, target_position);
    // }

    // ceres::Solver::Options options;
    // options.linear_solver_type = ceres::DENSE_QR;
    // options.minimizer_progress_to_stdout = true;

    // ceres::Solver::Summary summary;
    // ceres::Solve(options, &problem, &summary);

    // std::cout << summary.FullReport() << "\n";
    // std::cout << "Estimated target position: ["
    //           << target_position[0] << ", "
    //           << target_position[1] << ", "
    //           << target_position[2] << "]\n";
    // return Eigen::Vector3d(target_position[0], target_position[1], target_position[2]);
    return Eigen::Vector3d(CameraPosition[0](0), CameraPosition[0](1), -0.1);
    // return Eigen::Vector3d(1.0, 2.0, 3.0);
}


// Eigen::Vector3d DDXTJC::calculateWorldViewingtargGetWorldPosition(Eigen::VectorXd Z, std::vector<Eigen::Vector3d> camera_position,DDXTJC_Camera camera){
//     finished = false;
//     std::vector<Eigen::Vector3d> result_unit_vector = DDXTJC::calculate_unit_vectors(camera,Z);
//     Eigen::Vector3d actor_position = calculatorActorPosition(camera_position, result_unit_vector);
//     target_position_ = actor_position;
//     return actor_position;
// }

// void DDXTJC::update_Position(){
//     if (finished =  false)
//     {
        
//         ukf_ptr->Process(target_position,Z,time);
//         target_position = ukf_ptr->getMixState();
//     }
// }

/** ************************************************************************************************************** */

/**
 *  S2步骤的整体调用
 * 
 *  每个DDXTJC_Input里有
 *  uint64_t measure_time;
 *  Picture_Location picture_loc;
 *  Eigen::MatrixXd CoRotationMatrix;
 *  Eigen::Vector3d Camera_Location;
 * 
 */
bool DDXTJC::update_Position(Eigen::Vector3d &TargetPosition, uint64_t &meanTime,
                             std::vector<DDXTJC_Input> Inputs)
{
    std::vector<Eigen::Vector3d> CameraPositions;
    std::vector<Eigen::Vector3d> unitVectors;
    meanTime = 0;
    for(const auto &Input:Inputs)
    {
        Eigen::MatrixXd CoRotationMatrix = Input.CoRotationMatrix;
        Gimbal_Camera camera = Input.camera;
        Picture_Location picture_loc = Input.picture_loc;
        Eigen::Vector3d Camera_Location = Input.Camera_Location;
        Eigen::Vector3d v_C_i = calculate_unit_vector(camera, picture_loc);
        
        Eigen::Vector3d WorldViewingAxisUnitVector = calculateWorldViewingAxisUnitVector(v_C_i, CoRotationMatrix);
        CameraPositions.push_back(Camera_Location);
        unitVectors.push_back(WorldViewingAxisUnitVector);
        meanTime+=Input.measure_time;
    }   
    meanTime/=Inputs.size();
    // 需要考虑求解失败的处理
    TargetPosition = calculatorActorPosition(CameraPositions, unitVectors);
    // TargetPosition = Camera_Location
    if(abs(TargetPosition(2))<0.1)
    {
        TargetPosition(2) = 0.1;
    }
    return true;
}
